# 机械臂控制单例
import threading
from time import sleep

from PyQt5.QtCore import QTime
from loguru import logger

import utils
from DobotApi import DobotApi
import DobotDllType as dType
from singleton import singleton


@singleton
class Arm(object):
    _instance_lock = threading.Lock()
    interrupt_flag = False

    def __init__(self):
        self.__position = 0
        self.__duration = 0
        # self.target_pos = {"x": 0, "y": 0, "z": 0}
        self.target_pos = [0, 0, 0]

    def interrupt(self, flag):
        self.interrupt_flag = flag

    def is_interrupted(self):
        return self.interrupt_flag

    def home(self):
        # todo 处理失败情况
        dType.SetQueuedCmdClear(DobotApi().instance())
        # dType.SetHOMEParams(DobotApi().instance(), 250, 0, 50, 0, isQueued=1)
        print(dType.SetHOMECmd(DobotApi().instance(), temp=0, isQueued=0))
        return True

    def move_to(self, params):
        with self._instance_lock:
            mode = dType.PTPMode.PTPMOVJXYZMode
            if "mode" in params:
                mode = params["mode"]

            dType.SetPTPCmdEx(DobotApi().instance(), mode, params["x"], params["y"], params["z"], 0, 1)
            logger.debug(params)
            self.target_pos[0] = params["x"]
            self.target_pos[1] = params["y"]
            self.target_pos[2] = params["z"]

    def set_params(self, params):
        with self._instance_lock:
            # coord_p = params["CoordinateParams"]
            common_p = params["CommonParams"]
            joint_p = params["JointParams"]

            # dType.SetPTPCoordinateParams(DobotApi().instance(), coord_p[0], coord_p[1], coord_p[2], coord_p[3], isQueued=1)
            dType.SetPTPJointParams(DobotApi().instance(), joint_p[0], joint_p[1], joint_p[2], joint_p[3], joint_p[4], joint_p[5], joint_p[6], joint_p[7], isQueued=1)
            # dType.SetPTPCommonParams(DobotApi().instance(), common_p[0], common_p[1], isQueued=1)

    def get_pos(self):
        with self._instance_lock:
            x = dType.GetPoseEx(DobotApi().instance(), 1)
            y = dType.GetPoseEx(DobotApi().instance(), 2)
            z = dType.GetPoseEx(DobotApi().instance(), 3)
            return x, y, z

    def arrival(self):
        pos = [0, 0, 0]
        x, y, z = self.get_pos()
        pos[0] = x
        pos[1] = y
        pos[2] = z

        for index in range(3):
            # print(pos[index])
            if not utils.approximate(pos[index], self.target_pos[index], 10):  # 运行的位置与目标位置比对
                # if pos[index] != self.target_pos[index]:
                return False
        else:
            return True

    @property
    def position(self):
        return self.__position

    @position.setter
    def position(self, position):
        if isinstance(position, int):
            self.__position = position
        else:
            print("error not int")

    def do_action(self, params, action):
        if "CoordinateParams" in params and "CommonParams" in params:
            self.set_params(params)
        elif "io" in params:
            dType.SetIOMultiplexingEx(DobotApi().instance(), params["pin"], dType.GPIOType.GPIOTypeDO, 1)
            dType.SetIODOEx(DobotApi().instance(), params["pin"], params["value"], 1)
        elif "pwm" in params:
            dType.SetIOMultiplexingEx(DobotApi().instance(), params["pin"], dType.GPIOType.GPIOTypePWM, 1)
            dType.SetIOPWMEx(DobotApi().instance(), params["pin"], params["freq"], params["duty_ratio"], 1)
        else:
            self.move_to(params)

        if "wait_timeout" in params:
            dType.dSleep(params["wait_timeout"])

        return True

    def wait_action(self, params: tuple):
        wait_timeout = 10

        if "not_wait" in params:
            return True

        if "wait_timeout" in params:
            wait_timeout = params["wait_timeout"]

        timeout = QTime.currentTime().addSecs(wait_timeout)
        while not self.arrival():
            if self.is_interrupted():
                return False
            sleep(1)
            if QTime.currentTime() > timeout:
                logger.error("Arm Move Timeout!")
                return False

        return True
